from socket import timeout
import time
import 小车.舵机.serial as serial
import RPi.GPIO as GPIO
import atexit
import sys
import os
import threading

last_pwm_x=7.5
last_pwm_y=7.5
pwm_x_min=5.5
pwm_x_max=10.5
pwm_y_min=7.0
pwm_y_max=9.0
file_name_x="config_x.txt"
file_name_y="config_y.txt"
p_gpio_x=7
p_gpio_y=18
last_error_x=0
last_error_y=0
# the time of revice last face position.
last_time=0
def check_data(data):
    if len(data)<12:
        return False
    if data[0] != '#':
        return False
    if data[1] != 'X':
        return False    
    if int(data[2:6])<=0 or int(data[2:6])>=900:
        return False
    if data[6] != 'Y':
        return False
    if int(data[7:11])<=0 or int(data[7:11])>=1300:
        return False
    if data[11] != '!':
        return False
    return True

# def save_to_file_x(data):
#     try:
#         global file_name_x
#         file=open(file_name_x,'w+')
#         file.read()
#         file.write(data)
#         file.flush()
#         file.close()
#     finally:
#         if file:
#             file.close()
#     return
def save_to_file_y(data):
    try:
        global file_name_y
        file=open(file_name_y,'w+')
        file.read()
        file.write(data)
        file.flush()
        file.close()
    finally:
        if file:
            file.close()
    return
def init_pwm_set():
    global last_pwm_x,last_pwm_y,file_name_x,file_name_y
    try:
        # if os.path.exists(file_name_x):
        #     file_x=open(file_name_x,'r')
        #     pwm_x=file_x.readline()
        #     last_pwm_x=float(pwm_x)
        #     file_x.close()
        if os.path.exists(file_name_y):
            file_y=open(file_name_y,'r')
            pwm_y=file_y.readline()
            last_pwm_y=float(pwm_y)
            file_y.close()
    except Exception as e:
        print(e)
        # file_x.close()
        file_y.close()
    return

def init_pwm_last(p_gpio,last_pwm):
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    print('init_pwm_last',p_gpio,last_pwm)
    GPIO.setup(p_gpio,GPIO.OUT,initial=GPIO.LOW)
    pwm=GPIO.PWM(p_gpio,50)
    pwm.start(0)
    pwm.ChangeDutyCycle(last_pwm)
    time.sleep(0.02)
    pwm.stop()
    GPIO.cleanup(p_gpio)
    return

# def pid_x(x):
#     if abs(x-400)<150:
#         return 0
#     global last_error_x
#     Kp=-0.0002
#     Ki=0.0009
#     error_x=x-400
#     dif_pwm_x=Kp*(error_x-last_error_x)+Ki*error_x
#     last_error_x=error_x
#     return float(format(dif_pwm_x,'.4f'))

def pid_y(y):
    if abs(y-640)<150:
        return 0
    global last_error_y
    Kp=-0.00001
    Ki=0.0007
    error_y=640-y
    dif_pwm_y=Kp*(error_y-last_error_y)+Ki*error_y
    last_error_y=error_y
    return float(format(dif_pwm_y,'.4f'))

# def send_pwm_x(target_pwm):
#     global last_pwm_x,p_gpio_x
#     GPIO.setwarnings(False)
#     # stop last control,save last pwm and begin target pwm.
#     GPIO.cleanup(p_gpio_x)
#     save_to_file_x(str(last_pwm_x))
#     # begin new control and send pwm to target.
#     GPIO.setwarnings(False)
#     GPIO.setmode(GPIO.BOARD)
#     print('gpio',p_gpio_x,last_pwm_x,target_pwm,threading.current_thread().name)
#     GPIO.setup(p_gpio_x,GPIO.OUT,initial=GPIO.LOW)
#     pwm=GPIO.PWM(p_gpio_x,50)
#     pwm.start(0)
#     if last_pwm_x<target_pwm:
#         while last_pwm_x<=target_pwm:
#             pwm.ChangeDutyCycle(last_pwm_x)
#             print('pwm',p_gpio_x,last_pwm_x)
#             last_pwm_x=float(format(last_pwm_x+0.005,'.3f'))
#             time.sleep(0.005)
#     else:
#         while last_pwm_x>=target_pwm:
#             pwm.ChangeDutyCycle(last_pwm_x)
#             print('pwm',p_gpio_x,last_pwm_x)
#             last_pwm_x=float(format(last_pwm_x-0.005,'.3f'))
#             time.sleep(0.005)
#     time.sleep(0.02)
#     pwm.stop()
#     GPIO.cleanup(p_gpio_x)
#     save_to_file_x(str(last_pwm_x))
#     return

def send_pwm_y(target_pwm):
    global last_pwm_y,p_gpio_y
    GPIO.setwarnings(False)
    # stop last control,save last pwm and begin target pwm.
    GPIO.cleanup(p_gpio_y)
    save_to_file_y(str(last_pwm_y))
    # begin new control and send pwm to target.
    print('gpio',p_gpio_y,last_pwm_y,target_pwm,threading.current_thread().name)
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(p_gpio_y,GPIO.OUT,initial=GPIO.LOW)
    pwm_y=GPIO.PWM(p_gpio_y,50)   
    pwm_y.start(0)
    if last_pwm_y<target_pwm:
        while last_pwm_y<=target_pwm:
            pwm_y.ChangeDutyCycle(last_pwm_y)
            print('pwm',p_gpio_y,last_pwm_y)
            last_pwm_y=float(format(last_pwm_y+0.005,'.3f'))
            time.sleep(0.005)
    else:
        while last_pwm_y>=target_pwm:
            pwm_y.ChangeDutyCycle(last_pwm_y)
            print('pwm',p_gpio_y,last_pwm_y)
            last_pwm_y=float(format(last_pwm_y-0.005,'.3f'))
            time.sleep(0.005)
    time.sleep(0.02)
    pwm_y.stop()
    GPIO.cleanup(p_gpio_y)
    save_to_file_y(str(last_pwm_y))
    return
def deal_data(receive):
    # if received data is multiple,only deal with the last one. 
    data=receive
    arr=receive.split("\n")
    for i in range(len(arr)-1,0,-1):
        if check_data(arr[i]):
            data=arr[i]
            break
        else:
            continue
    if not check_data(data):
        return ""
    global last_time
    last_time=time.time()
    global last_pwm_x,last_pwm_y,pwm_x_min,pwm_x_max,pwm_y_min,pwm_y_max
    x=int(data[2:6])
    y=int(data[7:11])
    # get pid on the x axis.
    # pi_x=pid_x(x)
    # if pi_x!=0:
    #     pwm_x=last_pwm_x+pi_x
    #     if pwm_x<pwm_x_min:
    #         pwm_x=pwm_x_min
    #     elif pwm_x>pwm_x_max:
    #         pwm_x=pwm_x_max
    #     thread_pwm_x=threading.Thread(target=send_pwm_x,args=(pwm_x,),name="thread_pwm_x")
    #     thread_pwm_x.start()
    # get pid on the y axis.
    pi_y=pid_y(y)
    if pi_y!=0:
        pwm_y=last_pwm_y+pi_y
        if pwm_y<pwm_y_min:
            pwm_y=pwm_y_min
        elif pwm_y>pwm_y_max:
            pwm_y=pwm_y_max
        thread_pwm_y=threading.Thread(target=send_pwm_y,args=(pwm_y,),name="thread_pwm_y")
        thread_pwm_y.start()
    return data


def main():
    ser=serial.Serial(port="/dev/ttyAMA0", baudrate=9600,timeout=5)
    if not ser.isOpen():
        ser.open()
    init_pwm_set()
    ser.write("/dev/ttyAMA0 is open".encode("UTF-8"))
    data=str(last_pwm_x)+"&"+str(last_pwm_y)
    ser.write(data.encode("UTF-8"))
    atexit.register(GPIO.cleanup)
    global last_time
    # init_pwm_last(p_gpio_x,last_pwm_x)
    init_pwm_last(p_gpio_y,last_pwm_y)
    last_time=time.time()
    try:
        while True:
            size=ser.inWaiting()
            if size==0:
                current_time=time.time()
                if current_time-last_time>60:
                    print(time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()))
                    # init_pwm_last(p_gpio_x,last_pwm_x)
                    init_pwm_last(p_gpio_y,last_pwm_y)
                    last_time=current_time
                continue
            time.sleep(0.01)
            size=ser.inWaiting()
            receive=ser.read(size)
            print(receive,time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()))
            result=deal_data(receive)
            ser.flushInput()
            if len(result)>0:
                ser.flushOutput()
                ser.write(result.encode("UTF-8"))
                print("result",result)
            time.sleep(0.3)
    except Exception or KeyboardInterrupt as e:
        print('error',e)
        ser.close()
main()